#include "mechversionclient.h"
#include "utils.h"
#define CMD101_FORMAT "101,%d,0,%d,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf"
#define CMD101_REPLY_FORMAT "%d,%d"
#define CMD102_FORMAT "102,%d"

MechVersionClient::MechVersionClient()
{
    print_base_info();
}

MechVersionClient::~MechVersionClient()
{

}

int MechVersionClient::init()
{
    mm_sync_client.init(localPort, localIP);
    if(!mm_sync_client.connect())
    {
        LOG(ERROR) << "mm_sync_client connect to server failed!!!\n";
        return false;
    }
}

void MechVersionClient::print_base_info()
{
    LOG(INFO) << "mm-version server ip:" << localIP <<"; port :" << localPort<< std::endl;
}

int MechVersionClient::mm_cmd_run_version(int project_id, MECH_VERSION_POSE joint_pos, MECH_VERSION_POSE tcp_pose)
{
    if(-1 == mm_run_vis(project_id, 0, MECH_POSE_TYPE_EYE_IN_HAND,joint_pos,tcp_pose)){
        LOG(ERROR)<< "Run mm_run_vis error!";
        return -1;
    }
    return 0;
}

int MechVersionClient::mm_cmd_receive_verion(int project_id, std::vector<MECH_VERSION_POSE> &poses)
{
    std::string recv_str;
    if(-1 == mm_receive_vis(project_id, recv_str)){
        LOG(ERROR) << "mm_receive_vis err!";
        return -1;
    }
    if(-1 == parse_cmd_102_reply_str(recv_str, poses)){
        LOG(ERROR) << "parse_cmd_102_reply_str err!";
        return -1;
    }
    return 0;
}

int MechVersionClient::parse_cmd_102_reply_str(std::string replyStr, std::vector<MECH_VERSION_POSE> &poses)
{
    //LOG(INFO)<<"recv replyStr =" <<replyStr;
    std::vector<std::string> replys = stringSplit(replyStr, ',');
    if(replys.size()<5){
        return -1;
    }
    if(replys[0] != "102"){
        return -1;
    }
    if(replys[1] != "1100"){
        return -1;
    }

    int nRet = stoi(replys[2]);
    int nSize= stoi(replys[3]);
    for(int i=0;i<nSize;i++){
        MECH_VERSION_POSE pose;
        pose.p[0] = stod(replys[i*8]);
        pose.p[1] = stod(replys[i*8+1]);
        pose.p[2] = stod(replys[i*8+2]);
        pose.p[3] = stod(replys[i*8+3]);
        pose.p[4] = stod(replys[i*8+4]);
        pose.p[5] = stod(replys[i*8+5]);
        poses.push_back(pose);
    }
    return 0;
}

int MechVersionClient::mm_run_vis(int project_id, int except_pose, int vision_pose_type, MECH_VERSION_POSE joint_pos, MECH_VERSION_POSE tcp_pose)
{
    LOG(INFO)<<"mm_run_vis start!";

    char buf[128]={};
    snprintf(buf,sizeof(buf),"101,%d,%d,%d", project_id, except_pose, vision_pose_type);
    //    LOG(INFO)<<"buf =" << buf;
    std::string run_vis_cmd;
    std::string cmd = std::string(buf);
    //    LOG(INFO)<<"cmd =" << cmd;
    if(MECH_POSE_TYPE_EYE_TO_HAND_NO_POSE == vision_pose_type){
        run_vis_cmd = cmd;
    }else if(MECH_POSE_TYPE_EYE_TO_HAND == vision_pose_type){
        std::string joint_buf_cmd = mech_version_pose_to_string(joint_pos);
        run_vis_cmd = cmd + joint_buf_cmd;
    }else if(MECH_POSE_TYPE_EYE_IN_HAND_NO_POS == vision_pose_type){
        std::string tcp_pose_buf_cmd = mech_version_pose_to_string(tcp_pose);
        run_vis_cmd = cmd + ","+ tcp_pose_buf_cmd;
    }else if(MECH_POSE_TYPE_EYE_IN_HAND == vision_pose_type){
        std::string joint_buf_cmd = mech_version_pose_to_string(joint_pos);
        std::string tcp_pose_buf_cmd = mech_version_pose_to_string(tcp_pose);
        run_vis_cmd = cmd +"," +joint_buf_cmd + "," +tcp_pose_buf_cmd;
    }else{
        LOG(ERROR)<<"pose_type err, vision_pose_type = " <<vision_pose_type<<std::endl;
        return -1;
    }
    LOG(INFO)<<"send run_vis_cmd = " <<run_vis_cmd<<std::endl;

    int nRet = mm_sync_client.sync_send_data((char*)run_vis_cmd.c_str(),run_vis_cmd.length(),2000);
    if(-1 == nRet){
        LOG(ERROR)<<"version_sync_client sync_send_data err!" <<std::endl;
        return -1;
    }
    if(-1 == check_reply_cmd_status(mm_sync_client.mData,mm_sync_client.mDataLen,101,1102)){
        LOG(ERROR)<<"check_reply_cmd_status err!" <<std::endl;
        return -1;
    }

    LOG(INFO)<<"mm_run_vis end!";
    return 0;
}

std::string MechVersionClient::mech_version_pose_to_string(MECH_VERSION_POSE pose)
{
    char buf[64]={};
    snprintf(buf,sizeof(buf),"%.6lf,%.6lf,%.6lf,%.6lf,%.6lf,%.6lf", \
             pose.p[0], pose.p[1], pose.p[2], pose.p[3], pose.p[4], pose.p[5]);
    return std::string(buf);
}


int MechVersionClient::check_reply_cmd_status(char* data,int data_len,int cmd, int status)
{
    std::string replyStr = std::string(data,data_len);
    LOG(INFO)<<"recv replyStr = " <<replyStr<<std::endl;

    std::vector<std::string> strs = stringSplit(replyStr, ',');
    if(strs.size()<2){
        LOG(ERROR)<<"strs size err strs.size() =" <<strs.size()<<std::endl;
        return -1;
    }

    int nCmd = std::stoi(strs[0]);
    if(nCmd!=cmd){
        LOG(ERROR)<<"nCmd " << nCmd << "v.s cmd " << cmd<<std::endl;
        return -1;
    }

    int nStatus = std::stoi(strs[1]);
    if(nStatus != status){
        LOG(ERROR)<<"nStatus " <<nStatus << "v.s status " << status<<std::endl;
        return -1;
    }
    return 0;
}

int MechVersionClient::mm_receive_vis(int project_id,std::string& recv_array)
{
    char buf[64]={};
    snprintf(buf,sizeof(buf),"102,%d", project_id);
    std::string run_vis_cmd = buf;

    LOG(INFO)<<"send run_vis_cmd = " << run_vis_cmd<<std::endl;
    int nRet = mm_sync_client.sync_send_data((char*)run_vis_cmd.c_str(),run_vis_cmd.length(),2000);
    if(-1 == nRet){
        LOG(ERROR)<<"mm_sync_client sync_send_data err!" <<std::endl;
        return -1;
    }
    if(-1 == check_reply_cmd_status(mm_sync_client.mData,mm_sync_client.mDataLen,102,1100)){
        LOG(ERROR)<<"check_reply_cmd_status err!" <<std::endl;
        return -1;
    }

    recv_array = std::string(mm_sync_client.mData, mm_sync_client.mDataLen);
    return 0;
}

